#include "preprocess/image_process.h"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
namespace autodrive {
namespace perception {

void crop_resize(cv::Mat const& image, cv::Rect2i const& carea,
                 cv::Size2i const& rsize, cv::Mat& out) noexcept(false) {
  if (image.empty()) {
    throw std::runtime_error("source image is empty");
  }

  auto const& h{image.rows};
  auto const& w{image.cols};

  if (carea.x > w || carea.y > h || carea.height > h || carea.width > w) {
    auto err{"image process parameter is illegal, crop(x,y,height,width): " +
             std::to_string(carea.x) + "," + std::to_string(carea.y) + "," +
             std::to_string(carea.height) + "," + std::to_string(carea.width) +
             ".  image size(height,width): " + std::to_string(h) + ", " +
             std::to_string(w)};

    throw std::runtime_error(err);
  }

  // crop image
  auto const& cropImg{image(carea)};

  // resize image
  cv::resize(cropImg, out, rsize);
}

void bgr_to_nv12(cv::Mat const& src, cv::Mat& dst) noexcept(false) {
  if (src.empty()) {
    throw std::runtime_error("source image is empty");
  }
  // height
  auto const& h{src.rows};
  // width
  auto const& w{src.cols};

  cv::Mat yuvI420Img;
  cv::cvtColor(src, yuvI420Img, cv::COLOR_BGR2YUV_I420);

  dst = cv::Mat(h * 3 / 2, w, CV_8UC1);

  // copy y
  uint8_t* i420YPtr{yuvI420Img.ptr<uint8_t>()};
  uint8_t* nv12YPtr{dst.ptr<uint8_t>()};

  auto const& ySize{h * w * sizeof(uint8_t)};
  memcpy(nv12YPtr, i420YPtr, ySize);

  // copy uv
  auto const& uvSize{ySize / 4};
  uint8_t* uPtr = i420YPtr + ySize;
  uint8_t* vPtr = uPtr + uvSize;
  uint8_t* nv12UVPtr = nv12YPtr + ySize;
  for (size_t i = 0; i < uvSize; i++) {
    *nv12UVPtr++ = *uPtr++;
    *nv12UVPtr++ = *vPtr++;
  }
}

void nv12_to_bgr(char const* y_addr, char const* uv_addr,
                 cv::Size2i const& size, cv::Mat& img) noexcept(false) {
  if (nullptr == y_addr) {
    throw std::runtime_error("y_addr is nullptr");
  }

  if (nullptr == uv_addr) {
    throw std::runtime_error("uv_addr is nullptr");
  }

  auto const& height{size.height};
  auto const& width{size.width};

  cv::Mat yuvImg{cv::Mat((height * 3) / 2, width, CV_8UC1)};

  memcpy(yuvImg.data, y_addr, width * height);
  memcpy(yuvImg.data + height * width, uv_addr, width * height / 2);

  cv::cvtColor(yuvImg, img, cv::COLOR_YUV2BGR_NV12);
}

}  // namespace perception
}  // namespace autodrive